#include<stdio.h>
#include <errno.h>
#include <signal.h>
#include <stdio.h>
#include <string.h>
#include <sys/resource.h>
#include <sys/time.h>
#include <sys/types.h>
#include <unistd.h>
#include <time.h> /* clock_gettime() */
#include <sys/mman.h> /* mlockall() */
#include <sched.h> /* sched_setscheduler() */

#include"ecrt.h"

/****************************************************************************/

/** Task period in ns. */
#define PERIOD_NS   (1000000)

#define MAX_SAFE_STACK (8 * 1024) /* The maximum stack size which is
                                     guranteed safe to access without
                                     faulting */

/****************************************************************************/

/* Constants */
#define NSEC_PER_SEC (1000000000)
#define FREQUENCY (NSEC_PER_SEC / PERIOD_NS)

/****************************************************************************/

// EtherCAT
static ec_master_t *master = NULL;
static ec_master_state_t master_state = {};

static ec_domain_t *domainServoInput = NULL;
static ec_domain_state_t domainServoInput_state = {};
static ec_domain_t *domainServoOutput = NULL;
static ec_domain_state_t domainServoOutput_state = {};

static uint8_t *domainOutput_pd = NULL;
static uint8_t *domainInput_pd = NULL;

static ec_slave_config_t *sc_cooldrive = NULL;
static ec_slave_config_state_t sc_cooldrive_state = {};

/****************************************************************************/

// process data
#define CoolDrivePos0  0, 0
#define CoolDrivePos1  0, 1

#define CoolDrive 0x00000748, 0x0000000a


// offsets for PDO entries
static unsigned int  cntlwd1,cntlwd2,cntlwd3,cntlwd4;
static unsigned int  tarpos1,tarpos2,tarpos3,tarpos4;
static unsigned int  modper1,modper2,modper3,modper4;
static unsigned int  DB1_1,DB1_2,DB1_3,DB1_4;
static unsigned int  status1,status2,status3,status4;
static unsigned int  actpos1,actpos2,actpos3,actpos4;
static unsigned int  acttor1,acttor2,acttor3,acttor4;
static unsigned int  moddis1,moddis2,moddis3,moddis4;
static unsigned int  DB2_1,DB2_2,DB2_3,DB2_4;
static unsigned int  errcode1,errcode2,errcode3,errcode4;
static unsigned int  errflo1,errflo2,errflo3,errflo4;

static int cur_status;
static int cur_mode;
// process data
ec_pdo_entry_reg_t domainServoOutput_regs[] = {
    {CoolDrivePos0, CoolDrive, 0x6040, 0x00, &cntlwd1, NULL},
    {CoolDrivePos0, CoolDrive, 0x607a, 0x00, &tarpos1, NULL},
    {CoolDrivePos0, CoolDrive, 0x6060, 0x00, &modper1, NULL},
    {CoolDrivePos0, CoolDrive, 0x27fe, 0x00, &DB1_1, NULL},
    {CoolDrivePos0, CoolDrive, 0x6840, 0x00, &cntlwd2, NULL},
    {CoolDrivePos0, CoolDrive, 0x687a, 0x00, &tarpos2, NULL},
    {CoolDrivePos0, CoolDrive, 0x6860, 0x00, &modper2, NULL},
    {CoolDrivePos0, CoolDrive, 0x2ffe, 0x00, &DB1_2, NULL},
    {CoolDrivePos1, CoolDrive, 0x6040, 0x00, &cntlwd3, NULL},
    {CoolDrivePos1, CoolDrive, 0x607a, 0x00, &tarpos3, NULL},
    {CoolDrivePos1, CoolDrive, 0x6060, 0x00, &modper3, NULL},
    {CoolDrivePos1, CoolDrive, 0x27fe, 0x00, &DB1_3, NULL},
    {CoolDrivePos1, CoolDrive, 0x6840, 0x00, &cntlwd4, NULL},
    {CoolDrivePos1, CoolDrive, 0x687a, 0x00, &tarpos4, NULL},
    {CoolDrivePos1, CoolDrive, 0x6860, 0x00, &modper4, NULL},
    {CoolDrivePos1, CoolDrive, 0x2ffe, 0x00, &DB1_4, NULL},
    {}
};
ec_pdo_entry_reg_t domainServoInput_regs[] = {
    {CoolDrivePos0, CoolDrive, 0x6041, 0x00, &status1, NULL},
    {CoolDrivePos0, CoolDrive, 0x6064, 0x00, &actpos1, NULL},
    {CoolDrivePos0, CoolDrive, 0x6077, 0x00, &acttor1, NULL},    
    {CoolDrivePos0, CoolDrive, 0x6061, 0x00, &moddis1, NULL},
    {CoolDrivePos0, CoolDrive, 0x27ff, 0x00, &DB2_1, NULL},
    {CoolDrivePos0, CoolDrive, 0x603f, 0x00, &errcode1, NULL},
    {CoolDrivePos0, CoolDrive, 0x60f4, 0x00, &errflo1, NULL},
    {CoolDrivePos0, CoolDrive, 0x6841, 0x00, &status2, NULL},
    {CoolDrivePos0, CoolDrive, 0x6864, 0x00, &actpos2, NULL},
    {CoolDrivePos0, CoolDrive, 0x6877, 0x00, &acttor2, NULL},    
    {CoolDrivePos0, CoolDrive, 0x6861, 0x00, &moddis2, NULL},
    {CoolDrivePos0, CoolDrive, 0x2fff, 0x00, &DB2_2, NULL},
    {CoolDrivePos0, CoolDrive, 0x683f, 0x00, &errcode2, NULL},
    {CoolDrivePos0, CoolDrive, 0x68f4, 0x00, &errflo2, NULL},
    {CoolDrivePos1, CoolDrive, 0x6041, 0x00, &status3, NULL},
    {CoolDrivePos1, CoolDrive, 0x6064, 0x00, &actpos3, NULL},
    {CoolDrivePos1, CoolDrive, 0x6077, 0x00, &acttor3, NULL},    
    {CoolDrivePos1, CoolDrive, 0x6061, 0x00, &moddis3, NULL},
    {CoolDrivePos1, CoolDrive, 0x27ff, 0x00, &DB2_3, NULL},
    {CoolDrivePos1, CoolDrive, 0x603f, 0x00, &errcode3, NULL},
    {CoolDrivePos1, CoolDrive, 0x60f4, 0x00, &errflo3, NULL},
    {CoolDrivePos1, CoolDrive, 0x6841, 0x00, &status4, NULL},
    {CoolDrivePos1, CoolDrive, 0x6864, 0x00, &actpos4, NULL},
    {CoolDrivePos1, CoolDrive, 0x6877, 0x00, &acttor4, NULL},    
    {CoolDrivePos1, CoolDrive, 0x6861, 0x00, &moddis4, NULL},
    {CoolDrivePos1, CoolDrive, 0x2fff, 0x00, &DB2_4, NULL},
    {CoolDrivePos1, CoolDrive, 0x683f, 0x00, &errcode4, NULL},
    {CoolDrivePos1, CoolDrive, 0x68f4, 0x00, &errflo4, NULL},
    {}
};


/* Master 0, Slave 0, "CoolDrive RD"
 * Vendor ID:       0x00000748
 * Product code:    0x0000000a
 * Revision number: 0x00000002
 */

ec_pdo_entry_info_t slave_0_pdo_entries[] = {
    {0x6040, 0x00, 16},
    {0x607a, 0x00, 32},
    {0x6060, 0x00, 8},
    {0x27fe, 0x00, 8},
    {0x6840, 0x00, 16},
    {0x687a, 0x00, 32},
    {0x6860, 0x00, 8},
    {0x2ffe, 0x00, 8},
    {0x6041, 0x00, 16},
    {0x6064, 0x00, 32},
    {0x6077, 0x00, 16},
    {0x6061, 0x00, 8},
    {0x27ff, 0x00, 8},
    {0x603f, 0x00, 16},
    {0x60f4, 0x00, 32},
    {0x6841, 0x00, 16},
    {0x6864, 0x00, 32},
    {0x6877, 0x00, 16},
    {0x6861, 0x00, 8},
    {0x2fff, 0x00, 8},
    {0x683f, 0x00, 16},
    {0x68f4, 0x00, 32},
};

ec_pdo_info_t slave_0_pdos[] = {
    {0x1600, 4, slave_0_pdo_entries + 0},
    {0x1610, 4, slave_0_pdo_entries + 4},
    {0x1a00, 7, slave_0_pdo_entries + 8},
    {0x1a10, 7, slave_0_pdo_entries + 15},
};

ec_sync_info_t slave_0_syncs[] = {
    {0, EC_DIR_OUTPUT, 0, NULL, EC_WD_DISABLE},
    {1, EC_DIR_INPUT, 0, NULL, EC_WD_DISABLE},
    {2, EC_DIR_OUTPUT, 2, slave_0_pdos + 0, EC_WD_ENABLE},
    {3, EC_DIR_INPUT, 2, slave_0_pdos + 2, EC_WD_DISABLE},
    {0xff}
};

/* Master 0, Slave 1, "CoolDrive RD"
 * Vendor ID:       0x00000748
 * Product code:    0x0000000a
 * Revision number: 0x00000002
 */

ec_pdo_entry_info_t slave_1_pdo_entries[] = {
    {0x6040, 0x00, 16},
    {0x607a, 0x00, 32},
    {0x6060, 0x00, 8},
    {0x27fe, 0x00, 8},
    {0x6840, 0x00, 16},
    {0x687a, 0x00, 32},
    {0x6860, 0x00, 8},
    {0x2ffe, 0x00, 8},
    {0x6041, 0x00, 16},
    {0x6064, 0x00, 32},
    {0x6077, 0x00, 16},
    {0x6061, 0x00, 8},
    {0x27ff, 0x00, 8},
    {0x603f, 0x00, 16},
    {0x60f4, 0x00, 32},
    {0x6841, 0x00, 16},
    {0x6864, 0x00, 32},
    {0x6877, 0x00, 16},
    {0x6861, 0x00, 8},
    {0x2fff, 0x00, 8},
    {0x683f, 0x00, 16},
    {0x68f4, 0x00, 32},
};

ec_pdo_info_t slave_1_pdos[] = {
    {0x1600, 4, slave_1_pdo_entries + 0},
    {0x1610, 4, slave_1_pdo_entries + 4},
    {0x1a00, 7, slave_1_pdo_entries + 8},
    {0x1a10, 7, slave_1_pdo_entries + 15},
};

ec_sync_info_t slave_1_syncs[] = {
    {0, EC_DIR_OUTPUT, 0, NULL, EC_WD_DISABLE},
    {1, EC_DIR_INPUT, 0, NULL, EC_WD_DISABLE},
    {2, EC_DIR_OUTPUT, 2, slave_1_pdos + 0, EC_WD_ENABLE},
    {3, EC_DIR_INPUT, 2, slave_1_pdos + 2, EC_WD_DISABLE},
    {0xff}
};


/****************************************************************************/



/****************************************************************************/
/* Master 0, Slave 0, "ESTUN ProNet"
 * Vendor ID:       0x0000060a
 * Product code:    0x00000001
 * Revision number: 0x00000001
 */

ec_pdo_entry_info_t estun_pdo_entries[] = {
    //CoE CSP Mode (RX)
    {0x6040, 0x00, 16}, /* Controlword */
    {0x607a, 0x00, 32}, /* Target Position */
    {0x6060, 0x00, 8},  /*modes of operation*/
    {0x6098, 0x00, 8},  /*Homing method*/
    {0x6099, 0x01, 32},  /*Speed during search for switch*/
    {0x6099, 0x02, 32},  /*Speed during search for zero*/
    {0x609a, 0x00, 32},  /*homing_acceleration*/
    {0x607c, 0x00, 32},  /*home_offset*/
    //CoE CSP Mode (TX)
    {0x6041, 0x00, 16}, /* Statusword */
    {0x6064, 0x00, 32}, /* Position actual value */
    {0x6061, 0x00, 8},  /*modes of operation display*/
};

ec_pdo_info_t estun_pdos[] = {
    {0x1600, 8, estun_pdo_entries + 0}, /* CoE CSP Mode (RX) */
    {0x1a00, 3, estun_pdo_entries + 8}, /* CoE CSP Mode (TX) */
};

ec_sync_info_t estun_syncs[] = {
    {0, EC_DIR_OUTPUT, 0, NULL, EC_WD_DISABLE},
    {1, EC_DIR_INPUT, 0, NULL, EC_WD_DISABLE},
    {2, EC_DIR_OUTPUT, 1, estun_pdos + 0, EC_WD_ENABLE},
    {3, EC_DIR_INPUT, 1, estun_pdos + 1, EC_WD_DISABLE},
    {0xff}
};

/****************************************************************************/

// Analog in --------------------------

static ec_pdo_entry_info_t el3102_pdo_entries[] = {
    {0x3101, 1,  8}, // channel 1 status
    {0x3101, 2, 16}, // channel 1 value
    {0x3102, 1,  8}, // channel 2 status
    {0x3102, 2, 16}, // channel 2 value
    {0x6401, 1, 16}, // channel 1 value (alt.)
    {0x6401, 2, 16}  // channel 2 value (alt.)
};

static ec_pdo_info_t el3102_pdos[] = {
    {0x1A00, 2, el3102_pdo_entries},
    {0x1A01, 2, el3102_pdo_entries + 2}
};

static ec_sync_info_t el3102_syncs[] = {
    {2, EC_DIR_OUTPUT},
    {3, EC_DIR_INPUT, 2, el3102_pdos},
    {0xff}
};

// Analog out -------------------------

static ec_pdo_entry_info_t el4102_pdo_entries[] = {
    {0x3001, 1, 16}, // channel 1 value
    {0x3002, 1, 16}, // channel 2 value
};

static ec_pdo_info_t el4102_pdos[] = {
    {0x1600, 1, el4102_pdo_entries},
    {0x1601, 1, el4102_pdo_entries + 1}
};

static ec_sync_info_t el4102_syncs[] = {
    {2, EC_DIR_OUTPUT, 2, el4102_pdos},
    {3, EC_DIR_INPUT},
    {0xff}
};

// Digital out ------------------------

static ec_pdo_entry_info_t el2004_channels[] = {
    {0x3001, 1, 1}, // Value 1
    {0x3001, 2, 1}, // Value 2
    {0x3001, 3, 1}, // Value 3
    {0x3001, 4, 1}  // Value 4
};

static ec_pdo_info_t el2004_pdos[] = {
    {0x1600, 1, &el2004_channels[0]},
    {0x1601, 1, &el2004_channels[1]},
    {0x1602, 1, &el2004_channels[2]},
    {0x1603, 1, &el2004_channels[3]}
};

static ec_sync_info_t el2004_syncs[] = {
    {0, EC_DIR_OUTPUT, 4, el2004_pdos},
    {1, EC_DIR_INPUT},
    {0xff}
};

/*****************************************************************************/



